dgit.raspbian.org Git - pcl.git/blob

   1 /*
   2  * Software License Agreement (BSD License)
   3  *
   4  *  Point Cloud Library (PCL) - www.pointclouds.org
   5  *  Copyright (c) 2019-, Open Perception, Inc.
   6  *
   7  *  All rights reserved.
   8  *
   9  *  Redistribution and use in source and binary forms, with or without
  10  *  modification, are permitted provided that the following conditions
  11  *  are met:
  12  *
  13  *   * Redistributions of source code must retain the above copyright
  14  *     notice, this list of conditions and the following disclaimer.
  15  *   * Redistributions in binary form must reproduce the above
  16  *     copyright notice, this list of conditions and the following
  17  *     disclaimer in the documentation and/or other materials provided
  18  *     with the distribution.
  19  *   * Neither the name of the copyright holder(s) nor the names of its
  20  *     contributors may be used to endorse or promote products derived
  21  *     from this software without specific prior written permission.
  22  *
  23  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  24  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  25  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  26  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  27  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  28  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  29  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  30  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  31  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  32  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  33  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  34  *  POSSIBILITY OF SUCH DAMAGE.
  35  *
  36  */
  37
  38 #pragma once
  39
  40 #include <pcl/cloud_iterator.h>
  41
  42 //////////////////////////////////////////////////////////////////////////////////////////////
  43 template <typename PointSource, typename PointTarget, typename Scalar> inline void
  44 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
  45 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  46                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  47                              Matrix4 &transformation_matrix) const
  48 {
  49   const auto nr_points = cloud_src.points.size ();
  50   if (cloud_tgt.points.size () != nr_points)
  51   {
  52     PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs from target (%lu)!\n", nr_points, cloud_tgt.points.size ());
  53     return;
  54   }
  55
  56   ConstCloudIterator<PointSource> source_it (cloud_src);
  57   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  58   estimateRigidTransformation (source_it, target_it, transformation_matrix);
  59 }
  60
  61 //////////////////////////////////////////////////////////////////////////////////////////////
  62 template <typename PointSource, typename PointTarget, typename Scalar> void
  63 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
  64 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  65                              const std::vector<int> &indices_src,
  66                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  67                              Matrix4 &transformation_matrix) const
  68 {
  69   const auto nr_points = indices_src.size ();
  70   if (cloud_tgt.points.size () != nr_points)
  71   {
  72     PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ());
  73     return;
  74   }
  75
  76   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  77   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  78   estimateRigidTransformation (source_it, target_it, transformation_matrix);
  79 }
  80
  81
  82 //////////////////////////////////////////////////////////////////////////////////////////////
  83 template <typename PointSource, typename PointTarget, typename Scalar> inline void
  84 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
  85 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  86                              const std::vector<int> &indices_src,
  87                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  88                              const std::vector<int> &indices_tgt,
  89                              Matrix4 &transformation_matrix) const
  90 {
  91   const auto nr_points = indices_src.size ();
  92   if (indices_tgt.size () != nr_points)
  93   {
  94     PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
  95     return;
  96   }
  97
  98   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  99   ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
 100   estimateRigidTransformation (source_it, target_it, transformation_matrix);
 101 }
 102
 103 //////////////////////////////////////////////////////////////////////////////////////////////
 104 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 105 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 106 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
 107                              const pcl::PointCloud<PointTarget> &cloud_tgt,
 108                              const pcl::Correspondences &correspondences,
 109                              Matrix4 &transformation_matrix) const
 110 {
 111   ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
 112   ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
 113   estimateRigidTransformation (source_it, target_it, transformation_matrix);
 114 }
 115
 116 //////////////////////////////////////////////////////////////////////////////////////////////
 117 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 118 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 119 constructTransformationMatrix (const Vector6 &parameters,
 120                                Matrix4 &transformation_matrix) const
 121 {
 122   // Construct the transformation matrix from rotation and translation
 123   const Eigen::AngleAxis<Scalar> rotation_z (parameters (2), Eigen::Matrix<Scalar, 3, 1>::UnitZ ());
 124   const Eigen::AngleAxis<Scalar> rotation_y (parameters (1), Eigen::Matrix<Scalar, 3, 1>::UnitY ());
 125   const Eigen::AngleAxis<Scalar> rotation_x (parameters (0), Eigen::Matrix<Scalar, 3, 1>::UnitX ());
 126   const Eigen::Translation<Scalar, 3> translation (parameters (3), parameters (4), parameters (5));
 127   const Eigen::Transform<Scalar, 3, Eigen::Affine> transform = rotation_z * rotation_y * rotation_x *
 128                                                                translation *
 129                                                                rotation_z * rotation_y * rotation_x;
 130   transformation_matrix = transform.matrix ();
 131 }
 132
 133 //////////////////////////////////////////////////////////////////////////////////////////////
 134 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 135 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 136 estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
 137 {
 138   using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
 139   using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
 140
 141   Matrix6 ATA;
 142   Vector6 ATb;
 143   ATA.setZero ();
 144   ATb.setZero ();
 145   auto M = ATA.template selfadjointView<Eigen::Upper> ();
 146
 147   // Approximate as a linear least squares problem
 148   source_it.reset ();
 149   target_it.reset ();
 150   for (; source_it.isValid () && target_it.isValid (); ++source_it, ++target_it)
 151   {
 152     const Vector3 p (source_it->x, source_it->y, source_it->z);
 153     const Vector3 q (target_it->x, target_it->y, target_it->z);
 154     const Vector3 n1 (source_it->getNormalVector3fMap().template cast<Scalar>());
 155     const Vector3 n2 (target_it->getNormalVector3fMap().template cast<Scalar>());
 156     Vector3 n;
 157     if (enforce_same_direction_normals_)
 158     {
 159         if (n1.dot (n2) >= 0.)
 160             n = n1 + n2;
 161         else
 162             n = n1 - n2;
 163     }
 164     else
 165     {
 166         n = n1 + n2;
 167     }
 168
 169     if (!p.array().isFinite().all() ||
 170         !q.array().isFinite().all() ||
 171         !n.array().isFinite().all())
 172     {
 173       continue;
 174     }
 175
 176     Vector6 v;
 177     v << (p + q).cross (n), n;
 178     M.rankUpdate (v);
 179
 180     ATb += v * (q - p).dot (n);
 181   }
 182
 183   // Solve A*x = b
 184   const Vector6 x = M.ldlt ().solve (ATb);
 185
 186   // Construct the transformation matrix from x
 187   constructTransformationMatrix (x, transformation_matrix);
 188 }
 189
 190 //////////////////////////////////////////////////////////////////////////////////////////////
 191 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 192 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 193 setEnforceSameDirectionNormals (bool enforce_same_direction_normals)
 194 {
 195     enforce_same_direction_normals_ = enforce_same_direction_normals;
 196 }
 197
 198 //////////////////////////////////////////////////////////////////////////////////////////////
 199 template <typename PointSource, typename PointTarget, typename Scalar> inline bool
 200 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 201 getEnforceSameDirectionNormals ()
 202 {
 203     return enforce_same_direction_normals_;
 204 }